/*
 * File:    MatrixMath.c
 * Project:	HobbyPilotFirmware (HPF)
 * Author:  Wouter van Verre
 *
 * Created on February 20, 2011, 3:09 PM
 */

#include "Header.h"

void m3x3_MatrixMultiply(float * Mat1, float * Mat2, float * Dest) // Mat1 * Mat2 = Dest
{
	float temp;
	int n, i, x;

	for (n = 0; n < 3; n++)
	{
		for ( i = 0; i < 3; i++)
		{
			temp = 0;
			for (x = 0; x <3; x++)
				temp += (Mat1[3*n + x] * Mat2[3*x + i]);
			Dest[3*n + i] = temp;
		}
	}
}

void m3x3_MatrixSetRow(float * Matrix, float * Vector)
{
	Matrix[0] = Vector[0];
	Matrix[1] = Vector[1];
	Matrix[2] = Vector[2];
}
